#include "hybrid.h"

namespace Lio {

float Hybrid::vertices0[N_TRIANG][3][3]={
{
	{ 0.0f,-0.1f, 0.0f},
	{ 0.0f, 0.0f,-0.5f},
	{ 1.0f, 0.0f, 0.0f},
},
{
	{ 0.0f, 0.1f, 0.0f},
	{ 1.0f, 0.0f, 0.0f},
	{ 0.0f, 0.0f,-0.5f},
},
{
	{ 1.0f, 0.0f, 0.0f},
	{ 0.0f, 0.5f, 0.0f},
	{ 0.0f,-0.5f, 0.0f},
},
{
	{ 1.0f, 0.0f, 0.0f},
	{ 0.0f,-0.5f,-0.1f},
	{ 0.0f, 0.5f,-0.1f},
}};
float Hybrid::LightAmbient0[4]=	{ 0.5f, 0.5f, 0.5f, 1.0f};
float Hybrid::LightDiffuse0[4]=	{ 1.0f, 1.0f, 1.0f, 1.0f};
float Hybrid::LightPosition0[4]={ 0.0f, 0.0f, 2.0f, 1.0f};
CSTR Hybrid::captureNames[C_SIZE] = {
	_T("time"),
	_T("ecef_x"),_T("ecef_y"),_T("ecef_z"),
	_T("lat"),_T("lon"),_T("alt"),
	_T("vn_x"),_T("vn_y"),_T("vn_z"),
	_T("q_bn_0"),_T("q_bn_1"),_T("q_bn_2"),_T("q_bn_3"),
	_T("q_bi_0"),_T("q_bi_1"),_T("q_bi_2"),_T("q_bi_3"),
	_T("accel_x"),_T("accel_y"),_T("accel_z"),
	_T("gyro_roll"),_T("gyro_pitch"),_T("gyro_yaw"),
	_T("inert_a_x"),_T("inert_a_y"),_T("inert_a_z"),
	_T("wen_n_x"),_T("wen_n_y"),_T("wen_n_z")
};
int Hybrid::captureColors[C_SIZE] = {
	INT_GRAY,
	COLOR1,COLOR2,COLOR3,
	COLOR1,COLOR2,COLOR3,
	COLOR1,COLOR2,COLOR3,
	COLOR1,COLOR2,COLOR3,COLOR4,
	COLOR1,COLOR2,COLOR3,COLOR4,
	COLOR1,COLOR2,COLOR3,
	COLOR1,COLOR2,COLOR3,
	COLOR1,COLOR2,COLOR3,
	COLOR1,COLOR2,COLOR3
};
CSTR Hybrid::playNames[P_SIZE] = {
	_T("time"),
	
	_T("Error_Measured_Position"),_T("Error_Estimated_Position"),_T("Error_Corrected_Position"),
	_T("Error_Estimated_Speed"),_T("Error_Corrected_Speed"),
	_T("Error_Estimated_Orientation"),_T("Error_Corrected_Orientation"),

	_T("real_accel_x"),_T("real_accel_y"),_T("real_accel_z"),
	_T("input_accel_x"),_T("input_accel_y"),_T("input_accel_z"),
	_T("real_gyro_roll"),_T("real_gyro_pitch"),_T("real_gyro_yaw"),
	_T("input_gyro_roll"),_T("input_gyro_pitch"),_T("input_gyro_yaw"),

	_T("real_lat"),_T("real_lon"),_T("real_alt"),
	_T("estim_lat"),_T("estim_lon"),_T("estim_alt"),
	_T("measure_lat"),_T("measure_lon"),_T("measure_alt"),
	_T("correct_lat"),_T("correct_lon"),_T("correct_alt"),

	_T("real_x"),_T("real_y"),_T("real_z"),
	_T("estim_x"),_T("estim_y"),_T("estim_z"),
	_T("measure_x"),_T("measure_y"),_T("measure_z"),
	_T("correct_x"),_T("correct_y"),_T("correct_z"),
	_T("err_est_x"),_T("err_est_y"),_T("err_est_z"),
	_T("err_mes_x"),_T("err_mes_y"),_T("err_mes_z"),
	_T("err_cor_x"),_T("err_cor_y"),_T("err_cor_z"),

	_T("real_vnx"),_T("real_vny"),_T("real_vnz"),
	_T("estim_vnx"),_T("estim_vny"),_T("estim_vnz"),
	_T("correct_vnx"),_T("correct_vny"),_T("correct_vnz"),
	_T("err_est_vnx"),_T("err_est_vny"),_T("err_est_vnz"),
	_T("err_cor_vnx"),_T("err_cor_vny"),_T("err_cor_vnz"),
	
	_T("real_roll"),_T("real_pitch"),_T("real_yaw"),
	_T("estim_roll"),_T("estim_pitch"),_T("estim_yaw"),
	_T("correct_roll"),_T("correct_pitch"),_T("correct_yaw"),
	_T("err_est_roll"),_T("err_est_pitch"),_T("err_est_yaw"),
	_T("err_cor_roll"),_T("err_cor_pitch"),_T("err_cor_yaw"),
	
	_T("dlat"),_T("dlon"),_T("dalt"),
	_T("dvn_x"),_T("dvn_y"),_T("dvn_z"),
	_T("droll"),_T("dpitch"),_T("dyaw"),
	
	_T("noise_accel_x"),_T("noise_accel_y"),_T("noise_accel_z"),
	_T("noise_gyro_roll"),_T("noise_gyro_pitch"),_T("noise_gyro_yaw"),
	_T("noise_measure_lat"),_T("noise_measure_lon"),_T("noise_measure_alt"),
	_T("noise_input_trace")
};
int Hybrid::playColors[P_SIZE] = {
	INT_GRAY,
	
	COLOR1,COLOR2,COLOR3,
	COLOR2,COLOR3,
	COLOR2,COLOR3
};
int	Hybrid::objPlayColors[N_TRIANG]={INT_WBLUE,INT_WBLUE,INT_BLUE,INT_DBLUE};
int Hybrid::objSimColors[N_TRIANG]={INT_WRED,INT_WRED,INT_RED,INT_DRED};
void Hybrid::reshape()
{
	FlatApplication::reshape();
	reshapeGraphs();
}
void Hybrid::reshapeGraphs()
{
	if(fgCapture.visible&&fgPlay.visible)
	{
		fgCapture.setBounds(0.5f*window.width-10,10.0f,0.5f*window.width,0.5f*window.height-15);
		fgCapture.chnlReadLines=4;
		fgPlay.setBounds(0.5f*window.width-10,0.5f*window.height+5,0.5f*window.width,0.5f*window.height-15);
		fgPlay.chnlReadLines=4;
	}
	else if(fgCapture.visible)
	{
		fgCapture.setBounds(0.5f*window.width-10,10.0f,0.5f*window.width,(float)window.height-20);
		fgCapture.chnlReadLines=18;
	}
	else if(fgPlay.visible)
	{
		fgPlay.setBounds(0.5f*window.width-10,10.0f,0.5f*window.width,(float)window.height-20);
		fgPlay.chnlReadLines=18;
	}
	container.build();
}
void Hybrid::breaks()
{
	initVector(input_Wnb_b);
	initVector(ins.vn);
}
BOOL Hybrid::capture()
{
	if(captureIndex==captureCount)
		return FALSE;
	captureIndex++;
	
	// sample time
	captureData[C_TIME]=ins.time;
	// position in ECEF
	captureData[C_X]=ins.re[AXE_X];
	captureData[C_Y]=ins.re[AXE_Y];
	captureData[C_Z]=ins.re[AXE_Z];
	// position in LLA
	captureData[C_LAT]=ins.r[LLA_LAT];
	captureData[C_LON]=ins.r[LLA_LON];
	captureData[C_ALT]=ins.r[LLA_ALT];
	// navigation speed
	captureData[C_VNX]=ins.vn[AXE_X];
	captureData[C_VNY]=ins.vn[AXE_Y];
	captureData[C_VNZ]=ins.vn[AXE_Z];
	// oriantation in quaternion
	captureData[C_QBN_0]=ins.q[0];
	captureData[C_QBN_1]=ins.q[1];
	captureData[C_QBN_2]=ins.q[2];
	captureData[C_QBN_3]=ins.q[3];
	// drawing quaternion
	captureData[C_QBI_0]=qi[0];
	captureData[C_QBI_1]=qi[1];
	captureData[C_QBI_2]=qi[2];
	captureData[C_QBI_3]=qi[3];
	// acceleration input (accelerometers)
	captureData[C_ACCEL_X]=ins.ab[AXE_X];
	captureData[C_ACCEL_Y]=ins.ab[AXE_Y];
	captureData[C_ACCEL_Z]=ins.ab[AXE_Z];
	// rotation rate input (gyroscopes)
	captureData[C_GYRO_ROLL]=ins.Wib_b[ROT_P];
	captureData[C_GYRO_PITCH]=ins.Wib_b[ROT_Q];
	captureData[C_GYRO_YAW]=ins.Wib_b[ROT_R];
	// inertial acceleration
	captureData[C_INERT_A_X]=ins.aInert[AXE_X];
	captureData[C_INERT_A_Y]=ins.aInert[AXE_Y];
	captureData[C_INERT_A_Z]=ins.aInert[AXE_Z];
	// navigation frame rotation
	captureData[C_WEN_N_X]=ins.Wen_n[AXE_X];
	captureData[C_WEN_N_Y]=ins.Wen_n[AXE_Y];
	captureData[C_WEN_N_Z]=ins.Wen_n[AXE_Z];

	setCaptureData();
	fgCapture.graph.update();

	return TRUE;
}
BOOL Hybrid::play()
{
	double quat[4];

	if(counter>=captureIndex)
		return FALSE;

	playData[P_TIME]=ins.time;
	
	getCapture(counter,qi_play,						C_QBI_0,4);
	getCapture(counter,re_play,						C_X,3);
	getCapture(counter,playData+P_REAL_LAT,			C_LAT,3);
	getCapture(counter,playData+P_REAL_VNX,			C_VNX,3);
	getCapture(counter,playData+P_REAL_ACCEL_X,		C_ACCEL_X,3);
	getCapture(counter,playData+P_REAL_GYRO_ROLL,	C_GYRO_ROLL,3);
	getCapture(counter,quat,						C_QBN_0,4);

	quatToEuler(quat,playData+P_REAL_ROLL);

	// Generate Noise
	playData[P_NOISE_ACCEL_X]=		gm_a[AXE_X].next(dt);
	playData[P_NOISE_ACCEL_Y]=		gm_a[AXE_Y].next(dt);
	playData[P_NOISE_ACCEL_Z]=		gm_a[AXE_Z].next(dt);
	playData[P_NOISE_GYRO_ROLL]=	gm_w[ROT_P].next(dt);
	playData[P_NOISE_GYRO_PITCH]=	gm_w[ROT_Q].next(dt);
	playData[P_NOISE_GYRO_YAW]=		gm_w[ROT_R].next(dt);
	playData[P_NOISE_MES_LAT]=		gm_m[LLA_LAT].next(dt);
	playData[P_NOISE_MES_LON]=		gm_m[LLA_LON].next(dt);
	playData[P_NOISE_MES_ALT]=		gm_m[LLA_ALT].next(dt);

	// Apply input Noise
	input_accel[AXE_X]=		playData[P_INPUT_ACCEL_X]=		playData[P_REAL_ACCEL_X]+	playData[P_NOISE_ACCEL_X];
	input_accel[AXE_Y]=		playData[P_INPUT_ACCEL_Y]=		playData[P_REAL_ACCEL_Y]+	playData[P_NOISE_ACCEL_Y];
	input_accel[AXE_Z]=		playData[P_INPUT_ACCEL_Z]=		playData[P_REAL_ACCEL_Z]+	playData[P_NOISE_ACCEL_Z];
	input_gyro[ROT_P]=		playData[P_INPUT_GYRO_ROLL]=	playData[P_REAL_GYRO_ROLL]+	playData[P_NOISE_GYRO_ROLL];
	input_gyro[ROT_Q]=		playData[P_INPUT_GYRO_PITCH]=	playData[P_REAL_GYRO_PITCH]+playData[P_NOISE_GYRO_PITCH];
	input_gyro[ROT_R]=		playData[P_INPUT_GYRO_YAW]=		playData[P_REAL_GYRO_YAW]+	playData[P_NOISE_GYRO_YAW];
	input_measure[LLA_LAT]=	playData[P_MES_LAT]=			playData[P_REAL_LAT]+		playData[P_NOISE_MES_LAT];
	input_measure[LLA_LON]=	playData[P_MES_LON]=			playData[P_REAL_LON]+		playData[P_NOISE_MES_LON];
	input_measure[LLA_ALT]=	playData[P_MES_ALT]=			playData[P_REAL_ALT]+		playData[P_NOISE_MES_ALT];

	playData[P_EST_LAT]=ins.r[LLA_LAT];
	playData[P_EST_LON]=ins.r[LLA_LON];
	playData[P_EST_ALT]=ins.r[LLA_ALT];

	playData[P_EST_VNX]=ins.vn[AXE_X];
	playData[P_EST_VNY]=ins.vn[AXE_Y];
	playData[P_EST_VNZ]=ins.vn[AXE_Z];

	quatToEuler(ins.q,playData+P_EST_ROLL);

	playData[P_dLAT]=	dr[LLA_LAT]=ins.dX[0][0];
	playData[P_dLON]=	dr[LLA_LON]=ins.dX[1][0];
	playData[P_dALT]=	dr[LLA_ALT]=ins.dX[2][0];
	playData[P_dVNX]=	dv[AXE_X]=	ins.dX[3][0];
	playData[P_dVNY]=	dv[AXE_Y]=	ins.dX[4][0];
	playData[P_dVNZ]=	dv[AXE_Z]=	ins.dX[5][0];
	playData[P_dROLL]=	de[ROT_P]=	ins.dX[6][0];
	playData[P_dPITCH]=	de[ROT_Q]=	ins.dX[7][0];
	playData[P_dYAW]=	de[ROT_R]=	ins.dX[8][0];

	playData[P_COR_LAT]=	playData[P_EST_LAT]-	playData[P_dLAT];
	playData[P_COR_LON]=	playData[P_EST_LON]-	playData[P_dLON];
	playData[P_COR_ALT]=	playData[P_EST_ALT]-	playData[P_dALT];
	playData[P_COR_VNX]=	playData[P_EST_VNX]-	playData[P_dVNX];
	playData[P_COR_VNY]=	playData[P_EST_VNY]-	playData[P_dVNY];
	playData[P_COR_VNZ]=	playData[P_EST_VNZ]-	playData[P_dVNZ];
	playData[P_COR_ROLL]=	playData[P_EST_ROLL]-	playData[P_dROLL];
	playData[P_COR_PITCH]=	playData[P_EST_PITCH]-	playData[P_dPITCH];
	playData[P_COR_YAW]=	playData[P_EST_YAW]-	playData[P_dYAW];

	LLAtoECEF(playData+P_REAL_LAT,	playData+P_REAL_X);
	LLAtoECEF(playData+P_EST_LAT,	playData+P_EST_X);
	LLAtoECEF(playData+P_MES_LAT,	playData+P_MES_X);
	LLAtoECEF(playData+P_COR_LAT,	playData+P_COR_X);

	playData[P_ERR_EST_X]=		playData[P_EST_X]-		playData[P_REAL_X];
	playData[P_ERR_EST_Y]=		playData[P_EST_Y]-		playData[P_REAL_Y];
	playData[P_ERR_EST_Z]=		playData[P_EST_Z]-		playData[P_REAL_Z];

	playData[P_ERR_MES_X]=		playData[P_MES_X]-		playData[P_REAL_X];
	playData[P_ERR_MES_Y]=		playData[P_MES_Y]-		playData[P_REAL_Y];
	playData[P_ERR_MES_Z]=		playData[P_MES_Z]-		playData[P_REAL_Z];

	playData[P_ERR_EST_VNX]=	playData[P_EST_VNX]-	playData[P_REAL_VNX];
	playData[P_ERR_EST_VNY]=	playData[P_EST_VNY]-	playData[P_REAL_VNY];
	playData[P_ERR_EST_VNZ]=	playData[P_EST_VNZ]-	playData[P_REAL_VNZ];
	
	playData[P_ERR_EST_ROLL]=	playData[P_EST_ROLL]-	playData[P_REAL_ROLL];
	playData[P_ERR_EST_PITCH]=	playData[P_EST_PITCH]-	playData[P_REAL_PITCH];
	playData[P_ERR_EST_YAW]=	playData[P_EST_YAW]-	playData[P_REAL_YAW];
	
	playData[P_ERR_COR_X]=		playData[P_COR_X]-		playData[P_REAL_X];
	playData[P_ERR_COR_Y]=		playData[P_COR_Y]-		playData[P_REAL_Y];
	playData[P_ERR_COR_Z]=		playData[P_COR_Z]-		playData[P_REAL_Z];
	
	playData[P_ERR_COR_VNX]=	playData[P_COR_VNX]-	playData[P_REAL_VNX];
	playData[P_ERR_COR_VNY]=	playData[P_COR_VNY]-	playData[P_REAL_VNY];
	playData[P_ERR_COR_VNZ]=	playData[P_COR_VNZ]-	playData[P_REAL_VNZ];
	
	playData[P_ERR_COR_ROLL]=	playData[P_COR_ROLL]-	playData[P_REAL_ROLL];
	playData[P_ERR_COR_PITCH]=	playData[P_COR_PITCH]-	playData[P_REAL_PITCH];
	playData[P_ERR_COR_YAW]=	playData[P_COR_YAW]-	playData[P_REAL_YAW];
	
	// error moduls:
	playData[P_ERR_EST_ECEF]=	getLength(playData+P_ERR_EST_X);
	playData[P_ERR_MES_ECEF]=	getLength(playData+P_ERR_MES_X);
	playData[P_ERR_EST_VN]=		getLength(playData+P_ERR_EST_VNX);
	playData[P_ERR_EST_EULER]=	getLength(playData+P_ERR_EST_ROLL);
	playData[P_ERR_COR_ECEF]=	getLength(playData+P_ERR_COR_X);
	playData[P_ERR_COR_VN]=		getLength(playData+P_ERR_COR_VNX);
	playData[P_ERR_COR_EULER]=	getLength(playData+P_ERR_COR_ROLL);
	
	playData[P_NOISE_INPUT_TRACE]=ins.P.trace();

	updateIns(dt,input_accel,input_gyro);
	if(counter%correctPeriod==0)
		ins.correct(input_measure);

	counter++;
	return TRUE;
}
void Hybrid::updateIns(double sample, const double *input_accel, const double *input_gyro)
{
	ins.estimate(sample,input_accel,input_gyro);
	getNextQuat(qi,input_gyro,sample); // real inertial attitude
}
void Hybrid::clearTraject()
{
	fgCapture.graph.clear();
	fgPlay.graph.clear();
	limX.clear();
	limY.clear();
	limZ.clear();
	counter=0;
	captureIndex=0;
	cubeSize=DEF_CUBE_SIZE;
}
void Hybrid::destroy()
{
	InitInfo info;
	info.addDouble(_T("dev_r"),dev_r);
	info.addDouble(_T("dev_v"),dev_v);
	info.addDouble(_T("dev_q"),dev_q);
	info.addDouble(_T("dev_a"),dev_a);
	info.addDouble(_T("beta_a"),beta_a);
	info.addDouble(_T("dev_w"),dev_w);
	info.addDouble(_T("beta_w"),beta_w);
	info.addDouble(_T("dev_m"),dev_m_alt);
	info.addDouble(_T("beta_m"),beta_m);
	info.addInt(_T("insRecurs"),ins.recurs);
	info.addInt(_T("captureCount"),captureCount);
	info.addInt(_T("correctPeriod"),correctPeriod);
	info.addDouble(_T("dt"),dt);
	info.addInt(_T("kaccel"),kaccel);
	info.add(_T("trajectFileName"),trajectFileName);
	info.addInt(_T("seed"),seed);
	info.addInt(_T("left"),window.left);
	info.addInt(_T("top"),window.top);
	info.addInt(_T("width"),window.width);
	info.addInt(_T("height"),window.height);

	info.write(_T("init.txt"));
	if(captureIndex>1)
	{
		File file;
		if(file.open(trajectFileName,_T("wb")))
			save(file);
	}
	FlatApplication::destroy();
}
BOOL Hybrid::save(FILE *file)
{
	if(!file)
		return FALSE;
	DWORD header=C_HEADER,version=C_VERSION,size=C_SIZE,count=captureIndex;
	if(!fwrite(&header,4,1,file))
		return FALSE;
	if(!fwrite(&version,4,1,file))
		return FALSE;
	if(!fwrite(&size,4,1,file))
		return FALSE;
	if(!fwrite(&count,4,1,file))
		return FALSE;
	for(DWORD i=0;i<count;i++)
	{
		fgCapture.graph.getValues(captureData,i);
		if(fwrite(captureData,8,C_SIZE,file)!=C_SIZE)
			break;
	}
	fd.closeFile();
	return TRUE;
}
BOOL Hybrid::load(FILE *file)
{
	if(!file)
		return FALSE;
	captureIndex=0;
	DWORD header=0,version=0,size=0,count=0;
	float sample=0;
	if(!fread(&header,4,1,file)||header!=C_HEADER)
		return FALSE;
	if(!fread(&version,4,1,file)||version!=C_VERSION)
		return FALSE;
	if(!fread(&size,4,1,file)||size!=C_SIZE)
		return FALSE;
	if(!fread(&count,4,1,file))
		return 0;
	if(setCount(count))
	{
		for(;captureIndex<captureCount;captureIndex++)
		{
			if(fread(captureData,8,C_SIZE,file)!=C_SIZE)
				break;
			setCaptureData();
		}
	}
	adjustTraject();
	fgCapture.graph.update();
	fd.closeFile();
	initPlay();
	return TRUE;
}
void Hybrid::setCaptureData()
{
	fgCapture.graph.addValues();
	limX.add(captureData[C_X]);
	limY.add(captureData[C_Y]);
	limZ.add(captureData[C_Z]);
}
BOOL Hybrid::listen(WORD id, WORD code, const void *info)
{
	if(	bCapture.id==id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			if(bCapture.checked)
				setCount(format.toInt(tbCount.text,DEF_COUNT));
		}
		return TRUE;
	}
	if(	bCount.id==id||
		tbCount.id==id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setCount(format.toInt(tbCount.text,DEF_COUNT));
		}
		return TRUE;
	}
	if(	bNoise.id==id||
		tbDevR.id==id||
		tbDevV.id==id||
		tbDevQ.id==id||
		tbDevA.id==id||
		tbDevW.id==id||
		tbDevM.id==id||
		tbBetaA.id==id||
		tbBetaW.id==id||
		tbBetaM.id==id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setNoise(	format.toDouble(tbDevR.text,DEF_DEV_R),
						format.toDouble(tbDevV.text,DEF_DEV_V),
						format.toDouble(tbDevQ.text,DEF_DEV_Q),
						format.toDouble(tbDevA.text,DEF_DEV_A),
						format.toDouble(tbBetaA.text,DEF_BETA_A),
						format.toDouble(tbDevW.text,DEF_DEV_W),
						format.toDouble(tbBetaW.text,DEF_BETA_W),
						format.toDouble(tbDevM.text,DEF_DEV_M),
						format.toDouble(tbBetaM.text,DEF_BETA_M));
		}
		return TRUE;
	}
	if(bDef.id==id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setNoise(	DEF_DEV_R,DEF_DEV_V,DEF_DEV_Q,
						DEF_DEV_A,DEF_BETA_A,DEF_DEV_W,DEF_BETA_W,
						DEF_DEV_M,DEF_BETA_M);
		}
		return TRUE;
	}
	if(bRecurs.id==id)
	{
		if(code==FLAT_ACTION_DEFAULT)
			ins.recurs=bRecurs.checked;
		return TRUE;
	}
	if(	bPeriod.id==id||
		tbCorrect.id==id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setPeriods(	format.toInt(tbCorrect.text,DEF_COR_PERIOD));
			initPlay();
		}
		return TRUE;
	}
	if(	bSample.id==id||
		tbSample.id==id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setSampleTime(	format.toDouble(tbSample.text,DEF_SAMPLE_TIME));
			initPlay();
		}
		return TRUE;
	}
	if(bExit.id==id)
	{
		terminate();
		return TRUE;
	}
	if(bStep.id==id)
	{
		bStop.checked=TRUE;
		return TRUE;
	}
	if(bCapture.id==id)
	{
		if(bCapture.checked)
		{
			initSim();
			clearTraject();
			bPlay.checked=FALSE;
		}
		return TRUE;
	}
	if(bPlay.id==id)
	{
		if(bPlay.checked)
		{
			initPlay();
			bCapture.checked=FALSE;
		}
		return TRUE;
	}
	if(bReset.id==id)
	{
		initSim();
		clearTraject();
		return TRUE;
	}
	if(bScreen.id==id)
	{
		Image image;
		if(!screenshot(&image))
			message(_T("Error while creating screenshot..."));
		writeJPEG(_T("screen.jpg"),&image,100);
		return TRUE;
	}
	if(bGraphCapt.id==id)
	{
		fgCapture.visible=bGraphCapt.checked;
		reshapeGraphs();
		return TRUE;
	}
	if(bGraphPlay.id==id)
	{
		fgPlay.visible=bGraphPlay.checked;
		reshapeGraphs();
		return TRUE;
	}
	else if(id==bLoad.id)
	{
		if(!window.fullscreen)
		{
			bPlay.checked=FALSE;
			bCapture.checked=FALSE;
			fd.open();
			if(load(fd.readFile()))
			{
				adjustTraject();
				trajectFileName=fd.path;
			}
		}
		else
		{
			loadWait=TRUE;
			bFull.action();
		}
		return TRUE;
	}
	else if(id==bSaveBin.id)
	{
		if(captureIndex>1)
		{
			if(!window.fullscreen)
			{
				bPlay.checked=FALSE;
				bCapture.checked=FALSE;
				fd.save();
				save(fd.writeFile());
			}
			else
			{
				saveWait=TRUE;
				bFull.action();
			}
		}
		return TRUE;
	}
	else if(id==bExportGraph.id)
	{
		if(!window.fullscreen)
		{
			bPlay.checked=FALSE;
			bCapture.checked=FALSE;
			fd.save();
			FILE *file=fd.writeFile();
			if(file)
			{
				fgPlay.graph.save(file,TRUE);
				fd.closeFile();
			}
		}
		return TRUE;
	}
	if(bFull.id==id)
	{
		bCapture.checked=FALSE;
		bPlay.checked=FALSE;
		setFullscreen(bFull.checked);
		return TRUE;
	}
	return FALSE;
}
BOOL Hybrid::create ()				
{
	int i;

	FlatApplication::create();

	display.width=1024;
	display.height=768;
	display.bpp=32;

	memcpy(LightAmbient,LightAmbient0,sizeof(LightAmbient));
	memcpy(LightDiffuse,LightDiffuse0,sizeof(LightDiffuse));
	memcpy(LightPosition,LightPosition0,sizeof(LightPosition));
	
	noise_measure.setSize(3,3,TRUE);

	light=FALSE;
	pause=FALSE;
	pp=FALSE;
	tabp=FALSE;
	lp=FALSE;
	retp=FALSE;
	loadWait=FALSE;
	saveWait=FALSE;

	initVector(glTrans0,0.0f,0.0f,-6.0f);
	initVector(glTrans1,-2.2f,-0.5f,-8.0f);

	format.precision=4;
	//clearColor=INT_WHITE;
	fd.addFilter(_T("*"),_T("All Files"));

	fontSmall.setFace(_T("Courier New"));
	fontSmall.height=-12;

	fontMatrix.setFace(_T("Terminal"));
	fontMatrix.height=-10;

	add(&fontSmall);
	add(&fontMatrix);

	panel.bgColor=0;
	panel.borderColor=INT_GRAY;
	panel.bgColor=INT_WHITE;
	panel.borderLine=1;
	panel.setBounds(10,0,500,400);

	bNoise.text=_T("Noise");
	bNoise.borderLine=1;
	bNoise.borderColor=INT_GRAY;
	bNoise.activeLine=2;
	bNoise.font=&font;
	bNoise.fontColor=INT_BLACK;
	bNoise.margin.set(5,1);
	bNoise.bgColor=0xFFDDDDDD;
	bNoise.overColor=INT_WHITE;
	bNoise.pressColor=INT_WCYAN;
	bNoise.adjustSize=TRUE;
	bNoise.location.set(10,10);

	bDef.text=_T("Def");
	bDef.copy(bNoise);
	bDef.stick(DIR_RIGHT,5,0);

	lDevR.text=_T("Model: dr");
	lDevR.font=&font;
	lDevR.fontColor=INT_DGRAY;
	lDevR.margin.set(5,1);
	lDevR.adjustSize=TRUE;
	lDevR.stick(DIR_RIGHT,2,0);

	tbDevR.borderLine=1;
	tbDevR.borderColor=INT_GRAY;
	tbDevR.activeLine=2;
	tbDevR.font=&fontSmall;
	tbDevR.fontColor=INT_BLACK;
	tbDevR.margin.set(2,1);
	tbDevR.size.set(65,20);
	tbDevR.stick(DIR_RIGHT);

	lDevV.text=_T("dv");
	lDevV.copy(lDevR);
	lDevV.stick(DIR_RIGHT,2,0);

	tbDevV.copy(tbDevR);
	tbDevV.stick(DIR_RIGHT);

	lDevQ.text=_T("dq");
	lDevQ.copy(lDevR);
	lDevQ.stick(DIR_RIGHT,2,0);

	tbDevQ.copy(tbDevR);
	tbDevQ.stick(DIR_RIGHT);

	lDevA.text=_T("Input: da");
	lDevA.copy(lDevR);
	lDevA.stick(DIR_DOWN,0,5,&bNoise);

	tbDevA.copy(tbDevR);
	tbDevA.stick(DIR_RIGHT);

	lBetaA.text=_T("ba");
	lBetaA.copy(lDevR);
	lBetaA.stick(DIR_RIGHT,2,0);

	tbBetaA.copy(tbDevR);
	tbBetaA.stick(DIR_RIGHT);

	lDevW.text=_T("dw");
	lDevW.copy(lDevR);
	lDevW.stick(DIR_RIGHT,2,0);

	tbDevW.copy(tbDevR);
	tbDevW.stick(DIR_RIGHT);

	lBetaW.text=_T("bw");
	lBetaW.copy(lDevR);
	lBetaW.stick(DIR_RIGHT,2,0);

	tbBetaW.copy(tbDevR);
	tbBetaW.stick(DIR_RIGHT);

	lDevM.text=_T("Measure: dm");
	lDevM.copy(lDevR);
	lDevM.stick(DIR_DOWN,0,5,&lDevA);

	tbDevM.copy(tbDevR);
	tbDevM.stick(DIR_RIGHT);

	lBetaM.text=_T("bm");
	lBetaM.copy(lDevR);
	lBetaM.stick(DIR_RIGHT,2,0);

	tbBetaM.copy(tbDevR);
	tbBetaM.stick(DIR_RIGHT);

	bPeriod.text=_T("Period");
	bPeriod.copy(bNoise);
	bPeriod.stick(DIR_DOWN,0,5,&lDevM);

	lCorrect.text=_T("Cor");
	lCorrect.copy(lDevR);
	lCorrect.stick(DIR_RIGHT,2,0);

	tbCorrect.copy(tbDevR);
	tbCorrect.size.x=45;
	tbCorrect.stick(DIR_RIGHT);

	bSample.text=_T("Sample");
	bSample.copy(bNoise);
	bSample.stick(DIR_RIGHT,5,0);

	tbSample.copy(tbDevR);
	tbSample.size.x=55;
	tbSample.stick(DIR_RIGHT,2);
	
	bRecurs.text=_T("Recurs");
	bRecurs.copy(bNoise);
	bRecurs.checkable=TRUE;
	bRecurs.stick(DIR_RIGHT,5,0);

	bCapture.text=_T("Capture");
	bCapture.copy(bNoise);
	bCapture.checkable = TRUE;
	bCapture.bgColor=INT_WYELLOW;
	bCapture.stick(DIR_DOWN,0,5,&bPeriod);

	bReset.text=_T("Reset");
	bReset.copy(bCapture);
	bReset.checkable=FALSE;
	bReset.stick(DIR_RIGHT,5,0);
	
	bCount.text=_T("Count");
	bCount.copy(bReset);
	bCount.stick(DIR_RIGHT,5,0);

	tbCount.copy(tbDevR);
	tbCount.size.x=65;
	tbCount.stick(DIR_RIGHT,5,0);

	bAdjust.text=_T("Adjust");
	bAdjust.copy(bCapture);
	bAdjust.checked=TRUE;
	bAdjust.stick(DIR_RIGHT,5,0);
	
	bAuto.text=_T("Auto");
	bAuto.copy(bCapture);
	bAuto.checked=TRUE;
	bAuto.stick(DIR_RIGHT,5,0);

	bPlay.text=_T("Play");
	bPlay.copy(bCapture);
	bPlay.bgColor=INT_WBLUE;
	bPlay.stick(DIR_DOWN,0,5,&bCapture);

	bStop.text=_T("Stop");
	bStop.copy(bPlay);
	bStop.stick(DIR_RIGHT,5,0);

	bStep.text=_T("Step");
	bStep.copy(bPlay);
	bStep.stick(DIR_RIGHT,5,0);

	bFast.text=_T("Fast");
	bFast.copy(bPlay);
	bFast.checked=TRUE;
	bFast.stick(DIR_RIGHT,5,0);

	bLoop.text=_T("Loop");
	bLoop.copy(bPlay);
	bLoop.stick(DIR_RIGHT,5,0);

	bSaveBin.text=_T("Save");
	bSaveBin.copy(bNoise);
	bSaveBin.bgColor=INT_WGREEN;
	bSaveBin.stick(DIR_RIGHT,5,0,&bLoop);

	bExportGraph.text=_T("Export");
	bExportGraph.copy(bSaveBin);
	bExportGraph.stick(DIR_RIGHT,5,0,&bSaveBin);

	bLoad.text=_T("Load");
	bLoad.copy(bSaveBin);
	bLoad.stick(DIR_RIGHT,5,0,&bExportGraph);

	bGraphCapt.text=_T("Graph Capt");
	bGraphCapt.copy(bCapture);
	bGraphCapt.bgColor=0xFFDDDDDD;
	bGraphCapt.stick(DIR_DOWN,0,5,&bPlay);

	bGraphPlay.text=_T("Graph Play");
	bGraphPlay.copy(bGraphCapt);
	bGraphPlay.checked=TRUE;
	bGraphPlay.stick(DIR_RIGHT,5,0);

	bFull.text=_T("FullScr");
	bFull.copy(bNoise);
	bFull.checkable=TRUE;
	bFull.stick(DIR_RIGHT,5,0,&bGraphPlay);

	bScreen.text=_T("PrtScr");
	bScreen.copy(bNoise);
	bScreen.stick(DIR_RIGHT,5,0,&bFull);

	bExit.text=_T("Exit");
	bExit.copy(bNoise);
	bExit.code=VK_ESCAPE;
	bExit.bgColor=INT_WRED;
	bExit.stick(DIR_RIGHT,5,0,&bScreen);

	fgCapture.bgColor=INT_GRAY;
	fgCapture.font=&fontSmall;
	fgCapture.fontColor=INT_BLACK;
	fgCapture.graph.grid.set(3,5);
	fgCapture.graph.margin=10;
	fgCapture.graph.smooth=TRUE;
	fgCapture.graph.marginChannel.set(15,100);
	fgCapture.graph.font=&fontSmall;
	fgCapture.graph.formatY.exposant=TRUE;
	fgCapture.graph.formatY.precision=6;
	fgCapture.graph.draw_to_index=TRUE;
	fgCapture.graph.shift=FALSE;
	fgCapture.margin.set(10,10);
	fgCapture.visible=bGraphCapt.checked;
	CSTR xchnl=captureNames[0];
	chnlCapt[0]=fgCapture.graph.addChannel(xchnl);
	for(i=1;i<C_SIZE;i++)
	{
		int c=captureColors[i];
		chnlCapt[i]=fgCapture.graph.addChannel(captureNames[i],xchnl,c?c:INT_BLACK);
	}
	fgCapture.graph.hideAll();

	fgPlay.copy(fgCapture);
	fgPlay.visible=bGraphPlay.checked;
	xchnl=playNames[0];
	chnlPlay[0]=fgPlay.graph.addChannel(xchnl);
	for(i=1;i<P_SIZE;i++)
	{
		int c=playColors[i];
		chnlPlay[i]=fgPlay.graph.addChannel(playNames[i],xchnl,c?c:INT_BLACK);
	}
	fgPlay.graph.hideAll();

	chnlPlay[P_ERR_COR_ECEF]->thick=2;
	chnlPlay[P_ERR_COR_VN]->thick=2;
	chnlPlay[P_ERR_COR_EULER]->thick=2;

	add(&panel);
	panel.addChild(&bNoise);
	panel.addChild(&bDef);
	panel.addChild(&lDevR);
	panel.addChild(&tbDevR);
	panel.addChild(&lDevV);
	panel.addChild(&tbDevV);
	panel.addChild(&lDevQ);
	panel.addChild(&tbDevQ);
	panel.addChild(&lDevA);
	panel.addChild(&tbDevA);
	panel.addChild(&lBetaA);
	panel.addChild(&tbBetaA);
	panel.addChild(&lDevW);
	panel.addChild(&tbDevW);
	panel.addChild(&lBetaW);
	panel.addChild(&tbBetaW);
	panel.addChild(&lDevM);
	panel.addChild(&tbDevM);
	panel.addChild(&lBetaM);
	panel.addChild(&tbBetaM);
	panel.addChild(&bPeriod);
	panel.addChild(&lCorrect);
	panel.addChild(&tbCorrect);
	panel.addChild(&bSample);
	panel.addChild(&tbSample);
	panel.addChild(&bRecurs);
	panel.addChild(&bCapture);
	panel.addChild(&bReset);
	panel.addChild(&bCount);
	panel.addChild(&tbCount);
	panel.addChild(&bAdjust);
	panel.addChild(&bAuto);
	panel.addChild(&bPlay);
	panel.addChild(&bStop);
	panel.addChild(&bStep);
	panel.addChild(&bFast);
	panel.addChild(&bLoop);
	panel.addChild(&bGraphCapt);
	panel.addChild(&bGraphPlay);
	panel.addChild(&bSaveBin);
	panel.addChild(&bExportGraph);
	panel.addChild(&bLoad);
	panel.addChild(&bFull);
	panel.addChild(&bScreen);
	panel.addChild(&bExit);

	add(&fgCapture);
	add(&fgPlay);

	setLight(light);
	
	init();
	return TRUE;
}
void Hybrid::init()
{
	captureCount=0;
	step_w=0.01;
	step_rot=0.1f;
	initVector(glRot,-60,0,-120);
	clearTraject();

	InitInfo info;
	CSTR ini = _T("init.txt");
	if(cmdToken.length>0 && cmdToken[0].length())
		ini = cmdToken[0];
	String tmp = _T("Hybrid");
	if(info.read(ini))
	{
		tmp += '(';
		tmp += ini;
		tmp += ')';
		setNoise(	info.getDouble(_T("dev_r"),DEF_DEV_R),
					info.getDouble(_T("dev_v"),DEF_DEV_V),
					info.getDouble(_T("dev_q"),DEF_DEV_Q),
					info.getDouble(_T("dev_a"),DEF_DEV_A),
					info.getDouble(_T("beta_a"),DEF_BETA_A),
					info.getDouble(_T("dev_w"),DEF_DEV_W),
					info.getDouble(_T("beta_w"),DEF_BETA_W),
					info.getDouble(_T("dev_m"),DEF_DEV_M),
					info.getDouble(_T("beta_m"),DEF_BETA_M));
		seed=info.getInt(_T("seed"),seed);
		bRecurs.checked=ins.recurs=info.getInt(_T("insRecurs"),DEF_RECURS);
		setCount(	info.getInt(_T("captureCount"),DEF_COUNT));
		setPeriods(	info.getInt(_T("correctPeriod"),DEF_COR_PERIOD));
		setSampleTime(info.getDouble(_T("dt"),DEF_SAMPLE_TIME));
		trajectFileName=info.get(_T("trajectFileName"),DEF_TRAJECT_FILE_NAME);
		kaccel=info.getInt(_T("kaccel"),64);
		window.left=info.getInt(_T("left"),window.left);
		window.top=info.getInt(_T("top"),window.top);
		window.width=info.getInt(_T("width"),window.width);
		window.height=info.getInt(_T("height"),window.height);
	}
	else
	{
		//message(_T("Failed to load ini file: \"%s\""), (CSTR)ini);
		setNoise(	DEF_DEV_R,DEF_DEV_V,DEF_DEV_Q,
					DEF_DEV_A,DEF_BETA_A,DEF_DEV_W,DEF_BETA_W,
					DEF_DEV_M,DEF_BETA_M);
		ins.recurs=DEF_RECURS;
		setCount(DEF_COUNT);
		setPeriods(DEF_COR_PERIOD);
		setSampleTime(DEF_SAMPLE_TIME);
		trajectFileName=DEF_TRAJECT_FILE_NAME;
		kaccel=64;
	}
	name=tmp;
	initSim();
	clearTraject();
	initPlay();
	File file;
	if(!file.open(trajectFileName))
	{
		//message(_T("Failed to open trajectory: \"%s\""), (CSTR)trajectFileName);
	}
	else if(!load(file))
	{
		//message(_T("Failed to load trajectory: \"%s\""), (CSTR)trajectFileName);
	}
	adjustTraject();
}
BOOL Hybrid::setCount(int newCount)
{
	int i;
	if(newCount<=0)
		return FALSE;
	if(newCount!=captureCount)
	{
		for(i=0;i<C_SIZE;i++)
			chnlCapt[i]->setSource(captureData+i,newCount);
		for(i=0;i<P_SIZE;i++)
			chnlPlay[i]->setSource(playData+i,newCount);
		tbCount.setText(format.toString(newCount));
		captureCount=newCount;
	}
	initSim();
	clearTraject();
	return TRUE;
}
BOOL Hybrid::setNoise(	double newDevR, double newDevV, double newDevQ,
						double newDevA, double newBetaA, double newDevW, double newBetaW,
						double newDevM, double newBetaM)
{
	if(	newDevR<0||newDevV<0||newDevQ<0||
		newDevA<0||newDevW<0||newBetaA<0||newBetaW<0||
		newDevM<0||newBetaM<0)
		return FALSE;

	seed=rand();
	format.precision=0;
	format.exposant=TRUE;
	format.precision=6;
	format.exposant=FALSE;
	tbDevR.setText(format.toString(newDevR));
	tbDevV.setText(format.toString(newDevV));
	tbDevQ.setText(format.toString(newDevQ));
	tbDevA.setText(format.toString(newDevA));
	tbDevW.setText(format.toString(newDevW));
	tbBetaA.setText(format.toString(newBetaA));
	tbBetaW.setText(format.toString(newBetaW));
	tbDevM.setText(format.toString(newDevM));
	tbBetaM.setText(format.toString(newBetaM));
	format.precision=4;
	
	dev_r=newDevR;
	dev_v=newDevV;
	dev_q=newDevQ;

	dev_a=newDevA;
	beta_a=newBetaA;
	dev_w=newDevW;
	beta_w=newBetaW;

	dev_m_lat=newDevM/A_EARTH;
	dev_m_lon=newDevM/A_EARTH;
	dev_m_alt=newDevM;
	beta_m=newBetaM;

	gm_a[AXE_X].init(dev_a,beta_a,dt);
	gm_a[AXE_Y].init(dev_a,beta_a,dt);
	gm_a[AXE_Z].init(dev_a,beta_a,dt);
	gm_w[ROT_P].init(dev_w,beta_w,dt);
	gm_w[ROT_Q].init(dev_w,beta_w,dt);
	gm_w[ROT_R].init(dev_w,beta_w,dt);
	gm_m[LLA_LAT].init(dev_m_lat,beta_m,dt);
	gm_m[LLA_LON].init(dev_m_lon,beta_m,dt);
	gm_m[LLA_ALT].init(dev_m_alt,beta_m,dt);

	updateNoise();

	initPlay();
	return TRUE;
}
void Hybrid::updateNoise()
{
	noise_dev[0]=dev_r/A_EARTH;
	noise_dev[1]=dev_r/A_EARTH;
	noise_dev[2]=dev_r;
	noise_dev[3]=dev_v;
	noise_dev[4]=dev_v;
	noise_dev[5]=dev_v;
	noise_dev[6]=dev_q;
	noise_dev[7]=dev_q;
	noise_dev[8]=dev_q;
	noise_dev[9]=dev_a;
	noise_dev[10]=dev_a;
	noise_dev[11]=dev_a;
	noise_dev[12]=dev_w;
	noise_dev[13]=dev_w;
	noise_dev[14]=dev_w;

	noise_measure.elems[0][0]=pow2(dev_m_lat); // var measure lat
	noise_measure.elems[1][1]=pow2(dev_m_lon); // var measure lon
	noise_measure.elems[2][2]=pow2(dev_m_alt); // var measure alt

	ins.setStateNoise(noise_dev,beta_a,beta_w);
	ins.setMeasureNoise(noise_measure);
}
void Hybrid::setPeriods(int newCorrectPeriod)
{
	if(newCorrectPeriod<1)
		newCorrectPeriod=1;
	tbCorrect.setText(format.toString(newCorrectPeriod));
	correctPeriod=newCorrectPeriod;
	updateNoise();
}
void Hybrid::setSampleTime(double newSampleTime)
{
	if(newSampleTime<=0)
		newSampleTime=DEF_SAMPLE_TIME;
	tbSample.setText(format.toString(newSampleTime));
	dt=newSampleTime;
}
BOOL Hybrid::glCreate ()
{
	FlatApplication::glCreate();
	glClearDepth(1.0f);
	glDepthFunc(GL_LEQUAL);
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	createLight(GL_LIGHT1,LightAmbient,LightDiffuse,LightPosition);
	//glEnable(GL_COLOR_MATERIAL);
	setLight(light);
	setClearColor(INT_WHITE);
	return TRUE;												
}
void Hybrid::glDestroy()
{
	FlatApplication::glDestroy();
}
void Hybrid::update()
{
	mShow=NULL;
	BOOL *keys=keyboard.keys;
	if(loadWait)
	{
		loadWait=FALSE;
		bLoad.action();
		return;
	}
	else if(saveWait)
	{
		saveWait=FALSE;
		bSaveBin.action();
		return;
	}
	if(!keyboard.alt)
	{
		if (keys ['L'])	
		{
			if(!lp)
			{
				lp=TRUE;
				light=!light;
				setLight(light);
				setUpdated(TRUE);
			}
		}
		else
		{
			lp = FALSE;
		}
		if (keys [VK_SPACE])
		{
			keys [VK_SPACE]=FALSE;
			bStep.checked=TRUE;
			bStop.checked=TRUE;
			setUpdated(TRUE);
		}
		if(keys['K'])
		{
			if(keys['0'])
			{
				kaccel=1;
				setUpdated(TRUE);
			}
			else if(keys['1'])
			{
				kaccel=2;
				setUpdated(TRUE);
			}
			else if(keys['2'])
			{
				kaccel=4;
				setUpdated(TRUE);
			}
			else if(keys['3'])
			{
				kaccel=8;
				setUpdated(TRUE);
			}
			else if(keys['4'])
			{
				kaccel=16;
				setUpdated(TRUE);
			}
			else if(keys['5'])
			{
				kaccel=32;
				setUpdated(TRUE);
			}
			else if(keys['6'])
			{
				kaccel=64;
				setUpdated(TRUE);
			}
			else if(keys['7'])
			{
				kaccel=128;
				setUpdated(TRUE);
			}
			else if(keys['8'])
			{
				kaccel=256;
				setUpdated(TRUE);
			}
			else if(keys['9'])
			{
				kaccel=512;
				setUpdated(TRUE);
			}
		}
		if(bGraphCapt.checked||bGraphPlay.checked)
		{
			if (keys[VK_OEM_MINUS]) 
			{
				cubeSize+=0.01f*cubeSize;
				setUpdated(TRUE);
			}
			else if (keys[VK_OEM_PLUS]) 
			{
				cubeSize-=0.01f*cubeSize;
				setUpdated(TRUE);
			}
		}
		else
		{
			if (keys[VK_OEM_MINUS]) 
			{
				glTrans0[AXE_Z]+=0.001f*glTrans0[AXE_Z];
				setUpdated(TRUE);
			}
			else if (keys[VK_OEM_PLUS])
			{
				glTrans0[AXE_Z]-=0.001f*glTrans0[AXE_Z];
				setUpdated(TRUE);
			}
		}
		if (keys['P'])					// Is Space Being Typed?
		{
			if(!pp)
			{
				pp=TRUE; // fp Becomes TRUE
				pause=!pause;
			}
		}
		else
		{
			pp=FALSE;
		}
		if(pause)
			return;
		if (keys [VK_F5])			
		{
			if(!retp)
			{
				retp=TRUE; // fp Becomes TRUE
				initSim();
			}
		}
		else
		{
			retp=FALSE;
		}
		if (keys ['B'])	// Breaks		
		{
			breaks();
		}
		if(bCapture.checked)
		{
			setUpdated(TRUE);
			initVector(input_accel);
			if(bAuto.checked)
			{
				input_accel[AXE_Z]=-G_EARTH;
			}
			if (keys[VK_UP])
				input_accel[AXE_X]+=kaccel;
			else if (keys[VK_DOWN])
				input_accel[AXE_X]-=kaccel;
			if (keys[VK_RIGHT])
				input_accel[AXE_Y]+=kaccel;
			else if (keys[VK_LEFT])
				input_accel[AXE_Y]-=kaccel;
			if (keys[VK_NEXT]) 
				input_accel[AXE_Z]+=kaccel;
			else if (keys[VK_PRIOR]) 
				input_accel[AXE_Z]-=kaccel;
			if (keys['Q']) 
				input_Wnb_b[ROT_P]-=step_w;
			if (keys['E']) 
				input_Wnb_b[ROT_P]+=step_w;
			if (keys['W']) 
				input_Wnb_b[ROT_Q]-=step_w;
			if (keys['S']) 
				input_Wnb_b[ROT_Q]+=step_w;
			if (keys['D']) 
				input_Wnb_b[ROT_R]+=step_w;
			if (keys['A']) 
				input_Wnb_b[ROT_R]-=step_w;
			ins.transRotation(input_Wnb_b,input_gyro);
			updateIns(dt,input_accel,input_gyro);
			if(!capture())
				bCapture.checked=FALSE;
			if(bAdjust.checked)
				adjustTraject();
		}
		else
		{
			if(bPlay.checked)
			{
				setUpdated(TRUE);
				if(!bStop.checked&&bFast.checked)
				{
					int i=0;
					BOOL stop=TRUE;
					while(play())
					{
						fgPlay.graph.addValues();
						if((++i)==FAST_PERIOD)
						{
							stop=FALSE;
							break;
						}
					}
					fgPlay.graph.update();
					if(stop)
					{
						if(bLoop.checked)
							initPlay();
						else
							bPlay.checked=FALSE;
					}
				}
				else if(!bStop.checked||bStep.checked)
				{
					if(play())
					{
						fgPlay.graph.addValues();
						fgPlay.graph.update();
					}
					else
					{
						if(bLoop.checked)
							initPlay();
						else
							bPlay.checked=FALSE;
					}
					bStep.checked=FALSE;
				}
			}
			if(keys['M'])
			{
				if (keys['1'])
				{
					mShow=&(ins.Q);
					mName=_T("Q");

				}
				else if (keys['2'])
				{
					mShow=&(ins.A);
					mName=_T("A");

				}
				else if (keys['3'])
				{
					mShow=&(ins.K);
					mName=_T("K");
				}
				else if (keys['4'])
				{
					mShow=&(ins.P);
					mName=_T("P");
				}
				else if (keys['5'])
				{
					mShow=&(ins.R);
					mName=_T("R");
				}
			}
			else
			{
				if (keys['Q'])
				{
					setUpdated(TRUE);
					glRot[AXE_Z]-=step_rot;
				}
				if (keys['E']) 
				{
					setUpdated(TRUE);
					glRot[AXE_Z]+=step_rot;
				}
				if (keys['W'])
				{
					setUpdated(TRUE);
					glRot[AXE_X]-=step_rot;
				}
				if (keys['S'])
				{
					setUpdated(TRUE);
					glRot[AXE_X]+=step_rot;
				}
				if (keys['D'])
				{
					setUpdated(TRUE);
					glRot[AXE_Y]+=step_rot;
				}
				if (keys['A'])
				{
					setUpdated(TRUE);
					glRot[AXE_Y]-=step_rot;
				}
			}
		}
	}
	if(mShow)
	{
		fgCapture.visible=FALSE;
		fgPlay.visible=FALSE;
		bGraphCapt.checked=FALSE;
		bGraphPlay.checked=FALSE;
	}
}
void Hybrid::initSim()
{
	initVector(input_accel);
	initVector(input_gyro);
	ins.init();
	initVector(input_Wnb_b,ins.Wnb_b);
	initVector(ecef0,ins.re);
	initQuat(qi,ins.q);
}
void Hybrid::initPlay()
{
	if(!bPlay.checked)
		return;
	// init noise
	SRAND(seed);
	getCapture(0,input_accel,C_ACCEL_X,3);
	getCapture(0,input_gyro,C_GYRO_ROLL,3);
	getCapture(0,q0_play,C_QBN_0,4);
	getCapture(0,r0_play,C_LAT,3);
	getCapture(0,v0_play,C_VNX,3);
	ins.init(r0_play,v0_play,q0_play,input_accel,input_gyro);
	initVector(ecef0,ins.re);
	initQuat(qi,ins.q);
	counter=0;
	
	for(int i=0;i<3;i++)
	{
		gm_a[i].x=0;
		gm_w[i].x=0;
		gm_m[i].rand();
	}
	fgPlay.graph.clear();
}
void Hybrid::draw2D()
{
	if(pause)
	{
		font.color=INT_RED;
		font.x= 0.5f*(window.width-font.getTextWidth(_T("PAUSED")));
		font.y= 0.5f*window.height;
		glPrint(&font,_T("PAUSED"));
	}
	fontSmall.color=INT_DGRAY;
	fontSmall.x=10;
	fontSmall.y=window.height-40.0f;
	glPrint(&fontSmall,_T("Captures: %d/%d, Time: %.2f s, Accel: %d, Cube: %.2lf m"), counter, captureIndex, ins.time, kaccel, cubeSize);
	fontSmall.y+=18;
	glPrint(&fontSmall,_T("Speed: %e m/s, Rotation: %e rad/s"), getLength(ins.vn), getLength(ins.Wnb_b));
	fontSmall.y+=18;
	if(!bGraphCapt.checked&&!bGraphPlay.checked&&mShow)
		printMatrix(&fontMatrix,mShow->elems,mShow->rows,mShow->cols,mName,10,400,65,20,&fontSmall,_T("% .2e"),3,INT_BLACK,INT_BLACK,INT_DRED,INT_BLUE);
}
void Hybrid::draw3D (void)
{
	if(bGraphCapt.checked||bGraphPlay.checked)
		glTranslatef(glTrans1[AXE_X], glTrans1[AXE_Y], glTrans1[AXE_Z]);
	else if(!mShow)
		glTranslatef(glTrans0[AXE_X], glTrans0[AXE_Y], glTrans0[AXE_Z]);
	else
		return;
	glRotate(glRot[AXE_X],glRot[AXE_Y],glRot[AXE_Z]);
	fillPolygon(FALSE);
	glPushMatrix();
	glRotatef(-90.0f,0,1,0);
	glRotatef(-90.0f,1,0,0);
	drawTraject();
	glPushMatrix();
		drawObject(qi,ins.re,objSimColors);
	glPopMatrix();
	if(!bCapture.checked)
	{
		glPushMatrix();
		drawObject(qi_play,re_play,objPlayColors);
		glPopMatrix();
	}
	glPopMatrix();
	setColor(INT_GRAY);
	drawParallel();
	fillPolygon(TRUE);	
}
void Hybrid::drawObject(double * qi, double * ecef, int * colors)
{
	rotObject(qi,N_TRIANG,vertices0,vertices);
	if(cubeSize)
		glTranslated(	(ecef[0]-ecef0[0])*spanReduct,
						(ecef[1]-ecef0[1])*spanReduct,
						(ecef[2]-ecef0[2])*spanReduct);
	glRotatef(-90.0f,0,1,0);
	glLineWidth(2.0f);
	glBegin(GL_TRIANGLES);
	for(int i=0;i<N_TRIANG;i++)
	{
		getNormalVector(vertices[i][0],vertices[i][1],vertices[i][2],normal);
		glNormal3fv(normal);
		setColor(colors[i]);
		glVertex3fv(vertices[i][0]);
		glVertex3fv(vertices[i][1]);
		glVertex3fv(vertices[i][2]);
	}
	glEnd();
	glLineWidth(1.0f);
}
void Hybrid::drawTraject()
{
	if(!cubeSize)
		return;
	setColor(INT_DGREEN);
	glPushAttrib(GL_ENABLE_BIT);
	glDisable(GL_LIGHTING);
	glDisable(GL_BLEND);
	glDisable(GL_TEXTURE_2D);
	glLineWidth(2.0f);
	glBegin(GL_LINE_STRIP);
	for(int i=0;i<captureIndex;i++)
		glVertex3d(	(chnlCapt[C_X]->elems[i]-ecef0[0])*spanReduct,
					(chnlCapt[C_Y]->elems[i]-ecef0[1])*spanReduct,
					(chnlCapt[C_Z]->elems[i]-ecef0[2])*spanReduct);
	glEnd();
	glLineWidth(1.0f);
	glPopAttrib();
}

};

int WINAPI _tWinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance, LPSTR lpCmdLine, int nCmdShow)
{
	Lio::Hybrid hybrid;
	return hybrid.main(hInstance,hPrevInstance,lpCmdLine,nCmdShow);
}/**/